/******************************************************************************
 *  Author:     Williams, Jacob (williamsj@msoe.edu)
 *  Class:      CE-3910
 *  Instructor: Dr. Barnekow
 *****************************************************************************/
#include "servo.h"
#include <avr/io.h>

// The current tilt in degrees
uint8_t currentTilt;

// The current pan in degrees
uint8_t currentPan;

/**
 * Initializes the Servos. This includes setting the Pins on PORTD used for
 * output and setting up Timer 1 for PWN generation
 */
void initServo(void) {

    // Set Pins 4 and 5 on PORTD for output
    DDRD = DDRD | (1 << PD4) | (1 << PD5);

    // Set up Timer 1 for PWM generation
    TCCR1A = (1 << COM1A1) | (1 << COM1A0) | (1 << COM1B1) | (1 << COM1B0)
            | (1 << WGM11);
    TCCR1B = (1 << WGM13) | (1 << CS11);
    setPan(90);
    setTilt(90);
    ICR1 = 10000;

}

/**
 * Gets the current Pan angle
 * @return The current pan angle in degrees
 */
uint8_t getPan() {

    return currentPan;

}

/**
 * Gets the current Tilt angle
 * @return the current tilt angle in degrees
 */
uint8_t getTilt() {

    return currentTilt;

}

/**
 * Sets the angle that the camera is panned to. This value will be restricted
 * to be between MAX_PAN and MIN_PAN
 * @param angle The new angle to set the pan to
 */
void setPan(uint8_t angle) {

    if (angle >= MIN_PAN && angle <= MAX_PAN) {

        currentPan = angle;

        OCR1A = 9400 - 10 * angle;

    } else if (angle > MAX_PAN) {

        currentPan = MAX_PAN;

        OCR1A = 9400 - 10 * MAX_PAN;

    } else if (angle < MIN_PAN) {

        currentPan = MIN_PAN;

        OCR1A = 9400 - 10 * MIN_PAN;

    }

}

/**
 * Sets the angle that the camera is tilted to. This value will be restricted
 * to be between MAX_TILT and MIN_TILT
 * @param angle The new angle the tilt will be set to.
 */
void setTilt(uint8_t angle) {

    if (angle >= MIN_TILT && angle <= MAX_TILT) {

        currentTilt = angle;

        OCR1B = 9400 - 10 * angle;

    } else if (angle > MAX_TILT) {

        currentTilt = MAX_TILT;

        OCR1B = 9400 - 10 * MAX_TILT;

    } else if (angle < MIN_TILT) {

        currentTilt = MIN_TILT;

        OCR1B = 9400 - 10 * MIN_TILT;

    }

}

